import cv2
import imutils
import numpy as np

from db_tools.db_handler import DbHandler
from settings import *
import settings
import time

from tools.config_params import ConfigParams

db = DbHandler()
limitPosition = db.get_produce_limit_position()
# 机械手坐标的极限位置:
ROBOT_LIMIT_X_MIN = limitPosition.get("ROBOT_LIMIT_X_MIN", 430)  # 机械手坐标X的最小值
ROBOT_LIMIT_X_MAX = limitPosition.get("ROBOT_LIMIT_X_MAX", 770)  # 机械手坐标X的最大值
ROBOT_LIMIT_Y_MIN = limitPosition.get("ROBOT_LIMIT_Y_MIN", -600)  # 机械手坐标Y的最小值
ROBOT_LIMIT_Y_Max = limitPosition.get("ROBOT_LIMIT_Y_Max", -190)  # 机械手坐标Y的最大值
areaFilterLeftRight = 10000
xPos1 = 1100
xPos2 = 1550
colorBlue = (255, 0, 0)
colorGreen = (0, 255, 0)
colorRed = (0, 0, 255)
erodeDilateTimes = 12
erodeKernel = (13,13)
cp = ConfigParams()


def image_show(name, image, showState=False):
    """
    显示图片,用来调试的时候显示图片,按照一定的比例进行缩放
    :param name:显示的图片窗口名称
    :param image:显示的图片
    :param showState:是否显示
    :return:
    """
    if showState:
        sizeScale = 4
        newWidth = int(image.shape[1] / sizeScale)
        newHeight = int(image.shape[0] / sizeScale)
        showImage = cv.resize(image, (newWidth, newHeight), cv.INTER_AREA)
        cv.namedWindow("{}".format(name))
        cv.imshow("{}".format(name), showImage)
        cv.waitKey(0)

def find_total_board_info_6pin(image):
    boardResSize = (0, 0)
    if settings.TemplateImage is not None:
        templateImage = get_image_used(settings.TemplateImage)
        imageSub = cv2.subtract(image.copy(), templateImage)
        imageSliceY = imageSub.copy()
    else:
        imageSliceY = image.copy()
    image_show("ImageOriginal", image)
    # Y轴方向上的截取位置
    cv.destroyAllWindows()
    imageColor = cv.cvtColor(image, cv.COLOR_GRAY2BGR)
    # 先把图像分成左右两个部分
    # 中间的两个型材位置
    # 将中间的部分填充为黑色.
    imageSliceY[:, xPos1:xPos2] = 0
    image_show("FillBlack", imageSliceY)
    # 获取平均亮度
    meanVal = int(np.mean(imageSliceY))
    print("平均亮度: {}".format(meanVal))
    if meanVal > 100:
        thresUsed = 90
    else:
        # 然后整个图去找轮廓
        thresUsed = 50
    T, board = cv.threshold(imageSliceY, thresUsed, 255, cv.THRESH_BINARY)
    image_show("BoardThres", board)
    kernel = cv.getStructuringElement(cv.MORPH_RECT, erodeKernel)
    board = cv.erode(board, kernel, iterations=erodeDilateTimes)
    image_show("BoardOpened", board)

    cnts = cv.findContours(board.copy(), cv.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
    cnts = imutils.grab_contours(cnts)

    largeContours = [cnt for cnt in cnts if cv.contourArea(cnt) > areaFilterLeftRight]
    print("ContourFinded = {}".format([cv2.contourArea(cnt) for cnt in largeContours]))

    if len(largeContours) >= 2:
        # largeContours = sorted(largeContours, key=cv.contourArea, reverse=True)[:2]  # 只取前面两个最大的
        centerPoints = [get_center_by_contour(contour) for contour in largeContours]
        centerXmin = 150
        centerXmax = board.shape[1] - 150
        centerYmin = 150
        centerYMax = board.shape[0] - 150
        centerPointsValid = [center for center in centerPoints if
                             (centerXmax > center[0] > centerXmin and centerYMax > center[1] > centerYmin)]
        print("centerPoints: {},centerPointsValid: {}".format(centerPoints, centerPointsValid))
        centerPoints = centerPointsValid
        # 过去掉假的中心点,边缘轮廓太低的去除掉
        for index, center in enumerate(centerPoints):
            anotherCenter = centerPoints[(index + 1) % len(centerPoints)]
            cv.line(board, center, anotherCenter, 255, 20)
            image_show("JointCenterLine", board)
        cnts = cv.findContours(board.copy(), cv.RETR_LIST, cv.CHAIN_APPROX_SIMPLE)
        cnts = imutils.grab_contours(cnts)
        cntFirstFinded = sorted(cnts, key=cv.contourArea, reverse=True)[0]
    elif len(largeContours) == 1:
        cntFirstFinded = largeContours[0]
    else:
        info = "没有找到任何的轮廓,考虑板子没有到位或者的光源坏了."
        return "error", info, None, None, imageColor, boardResSize
    rectPoints = np.int0(cv.boxPoints(cv.minAreaRect(cntFirstFinded)))
    leftTop, rightTop, rightBottom, leftBottom = get_rect_points_clockwise(rectPoints)
    mask = np.zeros(board.shape, np.uint8)
    board = cv.drawContours(mask, [np.array([leftTop, rightTop, rightBottom, leftBottom])], -1, 255, -1)
    board = cv.morphologyEx(board, cv.MORPH_DILATE, kernel, iterations=erodeDilateTimes)
    image_show("boardDilated", board)
    boardFinal = board.copy()
    moveWidth = int(get_two_point_distance(leftTop, rightTop) * 0.1)
    moveHeight = int(get_two_point_distance(leftTop, leftBottom) * 0.2)
    extend = 100

    boardFinal[leftTop[1] - extend:leftTop[1] + moveHeight, leftTop[0] - extend:leftTop[0] + moveWidth] = 0
    boardFinal[rightTop[1] - extend:rightTop[1] + moveHeight, rightTop[0] - moveWidth:rightTop[0] + extend] = 0
    boardFinal[leftBottom[1] - moveHeight:leftBottom[1] + extend, leftBottom[0] - extend:leftBottom[0] + moveWidth] = 0
    boardFinal[rightBottom[1] - moveHeight:rightBottom[1] + extend,
    rightBottom[0] - moveWidth:rightBottom[0] + extend] = 0
    image_show("FinalBoardAngleMoved", boardFinal)

    cnts = cv.findContours(boardFinal.copy(), cv.RETR_LIST, cv.CHAIN_APPROX_SIMPLE)
    cnts = imutils.grab_contours(cnts)
    largeContours = [cnt for cnt in cnts if cv.contourArea(cnt) > areaFilterLeftRight]
    if len(largeContours) >= 1:
        largeContours = sorted(largeContours, key=cv.contourArea, reverse=True)  # 只取前面两个最大的
        cntFinalFinded = largeContours[0]
    else:
        info = "没有找到任何的轮廓,考虑板子没有到位或者的光源坏了."
        return "error", info, None, None, imageColor, boardResSize

    # 然后就是四条边线重新去找,如何去找呢?就是要截取出来重新找
    box = cv.minAreaRect(cntFinalFinded)
    (cx, cy), (w, h), boxAngle = box
    if boxAngle > 70:
        angleNew = 90 - boxAngle
        box = (cx, cy), (h, w), angleNew

    if boxAngle < -70:
        angleNew = 90 + boxAngle
        box = (cx, cy), (h, w), angleNew

    rectPoints = np.int0(cv.boxPoints(cv.minAreaRect(cntFinalFinded)))
    imageShow = imageColor.copy()
    leftTop, rightTop, rightBottom, leftBottom = get_rect_points_clockwise(rectPoints)
    cv.drawContours(imageShow, [rectPoints], -1, colorGreen, 3)
    image_show("ContourFindedFinal", imageShow)

    topLineK, topLineB = get_line_k_and_b(leftTop, rightTop)
    bottomLineK, bottomLineB = get_line_k_and_b(leftBottom, rightBottom)

    leftLineK, leftLineB = get_line_k_and_b(leftTop, leftBottom)
    rightLineK, rightLineB = get_line_k_and_b(rightTop, rightBottom)

    leftTop = get_two_lines_intersection(leftLineK, leftLineB, topLineK, topLineB)
    rightTop = get_two_lines_intersection(rightLineK, rightLineB, topLineK, topLineB)
    leftBottom = get_two_lines_intersection(leftLineK, leftLineB, bottomLineK, bottomLineB)
    rightBottom = get_two_lines_intersection(rightLineK, rightLineB, bottomLineK, bottomLineB)
    print("四个顶点数据：", leftTop, rightTop, rightBottom, leftBottom)

    cv.drawContours(imageColor, [np.array([leftTop, rightTop, rightBottom, leftBottom])], -1, colorGreen, 3)
    image_show("FourLineFinalFinded", imageColor)

    leftAngle = get_angle_by_k(leftLineK)
    rightAngle = get_angle_by_k(rightLineK)

    if leftAngle < 0:
        leftAngle = leftAngle + 90
    else:
        leftAngle = leftAngle - 90

    if rightAngle < 0:
        rightAngle = rightAngle + 90
    else:
        rightAngle = rightAngle - 90

    # 根据左右图的坐标,计算大图的坐标:
    angleMean = round((leftAngle + rightAngle) / 2, 3)
    print("Angle: {},LeftAngle: {},RightAngle: {}".format(angleMean,leftAngle,rightAngle))

    boardWidth = round((get_two_point_distance(leftTop,rightTop) + get_two_point_distance(leftBottom,rightBottom)) / 2 / cp.mm2pix,2)
    boardHeight = round((get_two_point_distance(leftTop,leftBottom) + get_two_point_distance(rightTop,rightBottom)) / 2 / cp.mm2pix,2)
    boardResSize = (boardWidth,boardHeight)
    print("boardWidth: {},boardHeight: {}".format(boardWidth,boardHeight))

    if boardWidth <= cp.smallBoardRange:
        # 如果是小板就抓取中心点
        yOffset = int(boardWidth / 2 * cp.mm2pix)
    else:
        # 固定边的偏移值
        yOffset = int(cp.catchPosOffset6pin * cp.mm2pix)
    fixLineMid = get_two_points_middle(leftTop, leftBottom)
    # 固定边是liftLine,按照这个line做垂线
    if leftLineK is None:
        verK = 0
        verB = fixLineMid[1] - verK * fixLineMid[0]
        catchY = fixLineMid[1] + abs(yOffset * np.sin(trans_k_to_rad(verK)))
        catchX = verB
    elif leftLineK == 0:
        catchX = fixLineMid[0]
        catchY = fixLineMid[1] + yOffset
    elif leftLineK > 0:
        verK = 1 / leftLineK
        verB = fixLineMid[1] - verK * fixLineMid[0]
        catchY = fixLineMid[1] + abs(yOffset * np.sin(trans_k_to_rad(verK)))
        catchX = (catchY - verB) / verK
    else:
        verK = -1 / leftLineK
        verB = fixLineMid[1] - verK * fixLineMid[0]
        catchY = fixLineMid[1] + abs(yOffset * np.sin(trans_k_to_rad(verK)))
        catchX = (catchY - verB) / verK
    catchPos = (int(catchX), int(catchY))
    print(catchX, catchY)
    interSection5 = np.int0(fixLineMid)

    cv.line(imageColor, tuple(interSection5), catchPos, colorRed, 3)
    cv.circle(imageColor, catchPos, 5, colorBlue, -1)
    cv.putText(imageColor, "angle:{:.2f}".format(angleMean), (catchPos[0] - 50, catchPos[1] - 200),
               cv.FONT_HERSHEY_SIMPLEX, 2, colorBlue, 4)
    cv.putText(imageColor, "catchPos:{}".format(catchPos), (catchPos[0] - 50, catchPos[1] - 30),
               cv.FONT_HERSHEY_SIMPLEX, 2, colorBlue, 4)
    heightPos = get_two_points_middle(leftTop,leftBottom)
    cv.putText(imageColor,"H:{} mm".format(boardHeight),(int(heightPos[0]),int(heightPos[1])),
               cv.FONT_HERSHEY_SIMPLEX,2,colorBlue,4)
    print("Angle: {:.2f},catchPos:{}".format(angleMean, catchPos))

    image_show("TotalBoard", imageColor)

    catchPosRobot = get_module_catch_pos(catchPos)
    if ROBOT_LIMIT_X_MIN < catchPosRobot[0] < ROBOT_LIMIT_X_MAX and ROBOT_LIMIT_Y_MIN < catchPosRobot[1] < ROBOT_LIMIT_Y_Max and \
            abs(angleMean) < cp.errorAngleThres:
        return "ok", "识别成功!", catchPosRobot, angleMean, imageColor,boardResSize
    elif abs(angleMean) >= cp.errorAngleThres:
        return "error", "识别的角度太大,识别角度:{:.2f},阈值:{:.2f}".format(angleMean, cp.errorAngleThres), catchPosRobot, angleMean, imageColor,boardResSize
    else:
        info = "识别的坐标超过极限位置,识别的抓取坐标X:{:.2f},ok范围:({},{}),坐标Y:{:.2f},ok范围:({},{})".format(
            catchPosRobot[0], ROBOT_LIMIT_X_MIN, ROBOT_LIMIT_X_MAX, catchPosRobot[1], ROBOT_LIMIT_Y_MIN, ROBOT_LIMIT_Y_Max)
        return "error", info, catchPosRobot, angleMean, imageColor,boardResSize

def find_total_board_info_4pin(image):
    boardResSize = (0,0)
    if settings.TemplateImage is not None:
        templateImage = get_image_used(settings.TemplateImage)
        imageSub = cv2.subtract(image.copy(), templateImage)
        imageSliceY = imageSub.copy()
    else:
        imageSliceY = image.copy()
    image_show("ImageOriginal", image)
    # Y轴方向上的截取位置
    cv.destroyAllWindows()
    imageColor = cv.cvtColor(image, cv.COLOR_GRAY2BGR)
    # 先把图像分成左右两个部分
    # 中间的两个型材位置
    # 将中间的部分填充为黑色.
    imageSliceY[:, xPos1:xPos2] = 0
    image_show("FillBlack", imageSliceY)
    # 获取平均亮度
    meanVal = int(np.mean(imageSliceY))
    print("平均亮度: {}".format(meanVal))
    if meanVal > 100:
        thresUsed = 90
    else:
        # 然后整个图去找轮廓
        thresUsed = 50
    T, board = cv.threshold(imageSliceY, thresUsed, 255, cv.THRESH_BINARY)
    image_show("BoardThres", board)
    kernel = cv.getStructuringElement(cv.MORPH_RECT, erodeKernel)
    board = cv.erode(board,kernel,iterations=erodeDilateTimes)
    image_show("BoardOpened", board)

    cnts = cv.findContours(board.copy(),cv.RETR_LIST,cv2.CHAIN_APPROX_SIMPLE)
    cnts = imutils.grab_contours(cnts)

    largeContours = [cnt for cnt in cnts if cv.contourArea(cnt) > areaFilterLeftRight]
    print("ContourFinded = {}".format([cv2.contourArea(cnt) for cnt in largeContours]))

    if len(largeContours) >= 2:
        # largeContours = sorted(largeContours, key=cv.contourArea, reverse=True)[:2]  # 只取前面两个最大的
        centerPoints = [get_center_by_contour(contour) for contour in largeContours]
        centerXmin = 150
        centerXmax = board.shape[1] - 150
        centerYmin = 150
        centerYMax = board.shape[0] - 150
        centerPointsValid = [center for center in centerPoints if (centerXmax > center[0] > centerXmin and centerYMax > center[1] > centerYmin)]
        print("centerPoints: {},centerPointsValid: {}".format(centerPoints, centerPointsValid))
        centerPoints = centerPointsValid
        # 过去掉假的中心点,边缘轮廓太低的去除掉
        for index, center in enumerate(centerPoints):
            anotherCenter = centerPoints[(index + 1) % len(centerPoints)]
            cv.line(board, center, anotherCenter, 255, 20)
            image_show("JointCenterLine", board)
        cnts = cv.findContours(board.copy(), cv.RETR_LIST, cv.CHAIN_APPROX_SIMPLE)
        cnts = imutils.grab_contours(cnts)
        cntFirstFinded = sorted(cnts, key=cv.contourArea, reverse=True)[0]
    elif len(largeContours) == 1:
        cntFirstFinded = largeContours[0]
    else:
        info = "没有找到任何的轮廓,考虑板子没有到位或者的光源坏了."
        return "error", info, None, None, imageColor,boardResSize
    rectPoints = np.int0(cv.boxPoints(cv.minAreaRect(cntFirstFinded)))
    leftTop, rightTop, rightBottom, leftBottom = get_rect_points_clockwise(rectPoints)
    mask = np.zeros(board.shape,np.uint8)
    board = cv.drawContours(mask ,[np.array([leftTop,rightTop,rightBottom,leftBottom])],-1,255,-1)
    board = cv.morphologyEx(board, cv.MORPH_DILATE, kernel, iterations=erodeDilateTimes)
    image_show("boardDilated", board)
    boardFinal = board.copy()
    moveWidth = int(get_two_point_distance(leftTop, rightTop) * 0.1)
    moveHeight = int(get_two_point_distance(leftTop, leftBottom) * 0.2)
    extend = 100

    boardFinal[leftTop[1] - extend:leftTop[1] + moveHeight, leftTop[0] - extend:leftTop[0] + moveWidth] = 0
    boardFinal[rightTop[1] - extend:rightTop[1] + moveHeight, rightTop[0] - moveWidth:rightTop[0] + extend] = 0
    boardFinal[leftBottom[1] - moveHeight:leftBottom[1] + extend, leftBottom[0] - extend:leftBottom[0] + moveWidth] = 0
    boardFinal[rightBottom[1] - moveHeight:rightBottom[1] + extend, rightBottom[0] - moveWidth:rightBottom[0] + extend] = 0
    image_show("FinalBoardAngleMoved", boardFinal)

    cnts = cv.findContours(boardFinal.copy(), cv.RETR_LIST, cv.CHAIN_APPROX_SIMPLE)
    cnts = imutils.grab_contours(cnts)
    largeContours = [cnt for cnt in cnts if cv.contourArea(cnt) > areaFilterLeftRight]
    if len(largeContours) >= 1:
        largeContours = sorted(largeContours, key=cv.contourArea, reverse=True)  # 只取前面两个最大的
        cntFinalFinded = largeContours[0]
    else:
        info = "没有找到任何的轮廓,考虑板子没有到位或者的光源坏了."
        return "error", info, None, None, imageColor,boardResSize

    # 然后就是四条边线重新去找,如何去找呢?就是要截取出来重新找
    box = cv.minAreaRect(cntFinalFinded)
    (cx, cy), (w, h), boxAngle = box
    if boxAngle > 70:
        angleNew = 90 - boxAngle
        box = (cx, cy), (h, w), angleNew

    if boxAngle < -70:
        angleNew = 90 + boxAngle
        box = (cx, cy), (h, w), angleNew

    rectPoints = np.int0(cv.boxPoints(cv.minAreaRect(cntFinalFinded)))
    imageShow = imageColor.copy()
    leftTop, rightTop, rightBottom, leftBottom = get_rect_points_clockwise(rectPoints)
    cv.drawContours(imageShow, [rectPoints], -1, colorGreen, 3)
    image_show("ContourFindedFinal", imageShow)

    # cv.drawContours(totalBoardThres, cntFinalFinded, -1, 255, -1)

    # totalBoardNoAngle, totalBoardOffset = get_board_without_angle(image, box)
    # findEdgeExtend = int(2 * cp.mm2pix)
    # totalBoardExtendForFindEdge, _ = get_board_without_angle(image, box, findEdgeExtend)
    # totalBoardThresForFindEdge, _ = get_board_without_angle(totalBoardThres, box, findEdgeExtend)
    #
    # topLineK, topLineB = find_first_edge_line(image, totalBoardExtendForFindEdge, totalBoardThresForFindEdge, findEdgeExtend, totalBoardOffset, box)
    # if topLineK is None and topLineB is None:
    topLineK, topLineB = get_line_k_and_b(leftTop, rightTop)
    # bottomLineK, bottomLineB = find_third_edge_line(totalBoardExtendForFindEdge, totalBoardThresForFindEdge, findEdgeExtend, totalBoardOffset, box)
    # if bottomLineK is None and bottomLineB is None:
    bottomLineK, bottomLineB = get_line_k_and_b(leftBottom, rightBottom)

    # leftLineK, leftLineB = find_left_edge_line(totalBoardExtendForFindEdge, totalBoardThresForFindEdge, findEdgeExtend, totalBoardOffset, box)
    # if leftLineK is None and leftLineB is None:
    leftLineK, leftLineB = get_line_k_and_b(leftTop, leftBottom)

    # rightLineK, rightLineB = find_right_edge_line(totalBoardExtendForFindEdge, totalBoardThresForFindEdge, findEdgeExtend, totalBoardOffset, box)
    # if rightLineK is None and rightLineB is None:
    rightLineK, rightLineB = get_line_k_and_b(rightTop, rightBottom)

    leftTop = get_two_lines_intersection(leftLineK, leftLineB, topLineK, topLineB)
    rightTop = get_two_lines_intersection(rightLineK, rightLineB, topLineK, topLineB)
    leftBottom = get_two_lines_intersection(leftLineK, leftLineB, bottomLineK, bottomLineB)
    rightBottom = get_two_lines_intersection(rightLineK, rightLineB, bottomLineK, bottomLineB)
    print("四个顶点数据：", leftTop, rightTop, rightBottom, leftBottom)
    #
    # H, W = image.shape[:2]
    # # 找到这几个点的极值点,如果超过了图像的范围,就报错,认为坐标有错误
    # if leftTop[0] < 0 or leftTop[1] < 0 or rightTop[0] > W or rightTop[1] < 0 or leftBottom[0] < 0 or leftBottom[1] > H or rightBottom[0] > W or rightBottom[1] > H:
    #     info = "计算的轮廓四个顶点超出了图像边界,图像错误"
    #     return "error", info, None, None, imageColor

    cv.drawContours(imageColor, [np.array([leftTop, rightTop, rightBottom, leftBottom])], -1, colorGreen, 3)
    image_show("FourLineFinalFinded", imageColor)

    # leftK, leftB = get_line_k_and_b(leftTop, leftBottom)
    # rightK, rightB = get_line_k_and_b(rightTop, rightBottom)
    leftAngle = get_angle_by_k(leftLineK)
    rightAngle = get_angle_by_k(rightLineK)

    if leftAngle < 0:
        leftAngle = leftAngle + 90
    else:
        leftAngle = leftAngle - 90

    if rightAngle < 0:
        rightAngle = rightAngle + 90
    else:
        rightAngle = rightAngle - 90

    # 根据左右图的坐标,计算大图的坐标:
    angleMean = round((leftAngle + rightAngle) / 2, 3)
    print("Angle: {},LeftAngle: {},RightAngle: {}".format(angleMean,leftAngle,rightAngle))
    # leftLine = [leftTop, leftBottom]
    # rightLine = [rightTop, rightBottom]
    # topK, topB = get_line_k_and_b(leftTop, rightTop)

    # topLine要做延长的
    # topK, topB = get_line_k_and_b(leftTopMid, rightTopMid)
    # bottomK, bottomB = get_line_k_and_b(leftBottomMid, rightBottomMid)
    # leftK, leftB = get_line_k_and_b(leftLine[0], leftLine[1])
    # rightK, rightB = get_line_k_and_b(rightLine[0], rightLine[1])
    #
    # leftTopIntersection = get_two_lines_intersection(leftK, leftB, topK, topB)
    # rightTopIntersection = get_two_lines_intersection(rightK, rightB, topK, topB)
    # leftBottomIntersection = get_two_lines_intersection(leftK, leftB, bottomK, bottomB)
    # rightBottomIntersection = get_two_lines_intersection(rightK, rightB, bottomK, bottomB)
    #
    # topLine = [leftTop, rightTop]
    # bottomLine = [leftBottom, rightBottom]
    #
    # cv.line(imageColor, tuple(leftLine[0]), tuple(leftLine[1]), colorGreen, thickness=3)
    # cv.line(imageColor, tuple(rightLine[0]), tuple(rightLine[1]), colorGreen, thickness=3)
    # cv.line(imageColor, tuple(topLine[0]), tuple(topLine[1]), colorGreen, thickness=5)
    # cv.line(imageColor, tuple(bottomLine[0]), tuple(bottomLine[1]), colorGreen, thickness=3)
    # image_show("TotalContourFinded", imageColor)

    boardWidth = round((get_two_point_distance(leftTop,rightTop) + get_two_point_distance(leftBottom,rightBottom)) / 2 / cp.mm2pix,2)
    boardHeight = round((get_two_point_distance(leftTop,leftBottom) + get_two_point_distance(rightTop,rightBottom)) / 2 / cp.mm2pix,2)
    boardResSize = (boardWidth,boardHeight)
    print("boardWidth: {},boardHeight: {}".format(boardWidth,boardHeight))

    if boardHeight <= cp.smallBoardRange:
        # 如果是小板就抓取中心点
        yOffset = int(boardHeight / 2 * cp.mm2pix)
    else:
        # 固定边的偏移值
        yOffset = int(cp.catchPosOffset4pin * cp.mm2pix)
    fixLineMid = get_two_points_middle(leftBottom, rightBottom)
    # 固定边是topLine,按照这个line做垂线
    if bottomLineK is None:
        verK = 0
        verB = fixLineMid[1] - verK * fixLineMid[0]
        catchY = fixLineMid[1] - abs(yOffset * np.sin(trans_k_to_rad(verK)))
        catchX = verB
    elif bottomLineK == 0:
        catchX = fixLineMid[0]
        catchY = fixLineMid[1] - yOffset
    else:
        verK = -1 / bottomLineK
        verB = fixLineMid[1] - verK * fixLineMid[0]
        catchY = fixLineMid[1] - abs(yOffset * np.sin(trans_k_to_rad(verK)))
        catchX = (catchY - verB) / verK

    catchPos = (int(catchX), int(catchY))
    print(catchX, catchY)
    interSection5 = np.int0(fixLineMid)

    cv.line(imageColor, tuple(interSection5), catchPos, colorRed, 3)
    cv.circle(imageColor, catchPos, 5, colorBlue, -1)
    cv.putText(imageColor, "angle:{:.2f}".format(angleMean), (catchPos[0] - 50, catchPos[1] - 200),
               cv.FONT_HERSHEY_SIMPLEX, 2, colorBlue, 4)
    cv.putText(imageColor, "catchPos:{}".format(catchPos), (catchPos[0] - 50, catchPos[1] - 30),
               cv.FONT_HERSHEY_SIMPLEX, 2, colorBlue, 4)
    heightPos = get_two_points_middle(leftTop,leftBottom)
    cv.putText(imageColor,"H:{} mm".format(boardHeight),(int(heightPos[0]),int(heightPos[1])),
               cv.FONT_HERSHEY_SIMPLEX,2,colorBlue,4)
    print("Angle: {:.2f},catchPos:{}".format(angleMean, catchPos))

    image_show("TotalBoard", imageColor)

    catchPosRobot = get_module_catch_pos(catchPos)
    if ROBOT_LIMIT_X_MIN < catchPosRobot[0] < ROBOT_LIMIT_X_MAX and ROBOT_LIMIT_Y_MIN < catchPosRobot[1] < ROBOT_LIMIT_Y_Max and \
            abs(angleMean) < cp.errorAngleThres:
        return "ok", "识别成功!", catchPosRobot, angleMean, imageColor,boardResSize
    elif abs(angleMean) >= cp.errorAngleThres:
        return "error", "识别的角度太大,识别角度:{:.2f},阈值:{:.2f}".format(angleMean, cp.errorAngleThres), catchPosRobot, angleMean, imageColor,boardResSize
    else:
        info = "识别的坐标超过极限位置,识别的抓取坐标X:{:.2f},ok范围:({},{}),坐标Y:{:.2f},ok范围:({},{})".format(
            catchPosRobot[0], ROBOT_LIMIT_X_MIN, ROBOT_LIMIT_X_MAX, catchPosRobot[1], ROBOT_LIMIT_Y_MIN, ROBOT_LIMIT_Y_Max)
        return "error", info, catchPosRobot, angleMean, imageColor,boardResSize

def get_module_catch_pos(catchPos):
    # 抓取的图像位置
    catchXpix, catchYPix = catchPos
    # 图像x轴和y轴对应机械手x轴和y轴,只是图像的x轴和机械手的x轴的正负是反的,y轴的正方向和机械手的正方向也是反的.

    imageOffsetX = (catchXpix - cp.centerImageX) / cp.mm2pix  # 转换成mm,坐标值越大的时候,这个偏移是正,到下面要减
    imageOffsetY = (catchYPix - cp.centerImageY) / cp.mm2pix  # 转换成mm,坐标值越大的时候,这个偏移是正,到下面要减

    print("像素偏移：", imageOffsetX, imageOffsetY)
    # 实际的抓板位置
    catchPosX = cp.centerModuleX - imageOffsetY
    catchPosY = cp.centerModuleY - imageOffsetX
    return [int(catchPosX), int(catchPosY)]

def get_two_points_middle(p1, p2):
    """
    获取两个点的中点坐标
    :param p1: 点1
    :param p2: 点2
    :return:
    """
    assert len(p1) == 2
    assert len(p2) == 2
    middleX = (p1[0] + p2[0]) / 2
    middleY = (p1[1] + p2[1]) / 2
    return round(middleX, 2), round(middleY, 2)

def get_line_k_and_b(p1, p2):
    """
    湖区某一条直线的斜率和斜距
    :param p1: 点1的坐标
    :param p2: 点2的坐标
    :return:
    """
    if p1[0] == p2[0]:
        # 垂直于x轴,没有k值
        return None, p1[0]
    else:
        k = (p2[1] - p1[1]) / (p2[0] - p1[0])
        b = p1[1] - k * p1[0]
        return round(k, 2), b

def get_two_point_distance(p1, p2):
    """
    获取两点之间的距离,保留小数点后两位
    :param p1: 点1
    :param p2: 点2
    :return:
    """
    assert len(p1) == 2
    assert len(p2) == 2
    ds = np.sqrt((p2[0] - p1[0]) ** 2 + (p2[1] - p1[1]) ** 2)
    return round(ds, 2)

def get_angle_by_k(k):
    """
    根据斜率k值,获取对应的角度值
    :param k: 线条的斜率k值
    :return:
    """
    if k == 0:
        return 0
    if k is None:
        return 90
    return np.arctan(k) * 180 / np.pi
def get_center_by_contour(contour):
    """
    根据轮廓点获取这个轮廓的中心点
    :param contour:
    :return:
    """
    return int(cv.minAreaRect(contour)[0][0]), int(cv.minAreaRect(contour)[0][1])
def get_rect_points_clockwise(rectPoints):
    """
    根据矩形的四个顶点,获取其顺时针方向的四个顶点
    :param rectPoints:
    :return:
    """
    rectPointsSorted = sorted(rectPoints, key=np.sum)
    leftTop = rectPointsSorted[0]
    rightBottom = rectPointsSorted[-1]

    if rectPointsSorted[1][0] > rectPointsSorted[2][0]:
        rightTop = rectPointsSorted[1]
        leftBottom = rectPointsSorted[2]
    else:
        rightTop = rectPointsSorted[2]
        leftBottom = rectPointsSorted[1]

    return tuple(leftTop), tuple(rightTop), tuple(rightBottom), tuple(leftBottom)

def get_two_lines_intersection(k1, b1, k2, b2):
    """
    获取平面上两条线的交点
    :param k1: 线1的k值
    :param b1: 线1的b值
    :param k2: 线2的k值
    :param b2: 线2的b值
    :return:
    """
    # 如果k相等,说明垂直无交点
    if k1 == k2:
        return None, None
    elif k1 is None and k2 is not None:
        return int(b1), int(k2 * b1 + b2)
    elif k2 is None and k1 is not None:
        return int(b2), int(k1 * b2 + b1)
    else:
        x = (b1 - b2) / (k1 - k2) * (-1)
        y = k1 * x + b1
    return int(x), int(y)

def get_image_used(image):
    # image = cv.cvtColor(image,cv.COLOR_RGB2GRAY)
    newImage = rotate_image_no_scale(image, -90)  # 时针旋转90度.
    # yStart, yEnd = [415, 3249]
    # newImage = newImage[yStart:yEnd,:]
    yStart, yEnd, xStart, xEnd = [481, 3429, 161, 2905]
    newImage = newImage[yStart:yEnd, xStart:xEnd]
    return newImage


def rotate_image_no_scale(imageSrc, rotateAngle):
    """
    无缩放的旋转图像的逻辑.旋转后的宽是原来的高,旋转后的高是原来的宽.
    :param imageSrc:要旋转的图片
    :param rotateAngle:旋转的角度,顺时针为负,逆时针为正.
    :return:返回旋转之后的图片
    """
    if len(imageSrc.shape) >= 3:
        # 先转换为灰度图
        imageSrc = cv.cvtColor(imageSrc, cv.COLOR_BGR2GRAY)

    height, width = imageSrc.shape[:2]
    # 根据旋转角度获取旋转矩阵
    if width > height:
        paddingSize = (width - height) // 2
        sizeUsed = (width, width)
        rotateCenter = (sizeUsed[0] // 2, sizeUsed[1] // 2)
        imagePadded = np.zeros(sizeUsed, dtype=np.uint8)
        imagePadded[paddingSize:paddingSize + height, :] = imageSrc
        rotatedM = cv.getRotationMatrix2D(rotateCenter, rotateAngle, 1.0)
        imageRotated = cv.warpAffine(imagePadded, rotatedM, sizeUsed)
        imageWithoutPadding = imageRotated[:, paddingSize:height + paddingSize]
    else:
        paddingSize = (height - width) // 2
        sizeUsed = (height, height)
        rotateCenter = (sizeUsed[0] // 2, sizeUsed[1] // 2)
        imagePadded = np.zeros(sizeUsed, dtype=np.uint8)
        imagePadded[:, paddingSize:paddingSize + width] = imageSrc
        rotatedM = cv.getRotationMatrix2D(rotateCenter, rotateAngle, 1.0)
        imageRotated = cv.warpAffine(imagePadded, rotatedM, sizeUsed)
        imageWithoutPadding = imageRotated[paddingSize:width + paddingSize, :]
    return imageWithoutPadding

def get_size_scale_image(imageSrc, sizeScale):
    """
    高宽同等比例缩放图像,缩放比例
    :param imageSrc: 原图像
    :param sizeScale: 缩放比例
    :return:
    """
    h, w = imageSrc.shape[:2]
    newWidth, newHeight = int(w / sizeScale), int(h / sizeScale)
    newImage = cv.resize(imageSrc, (newWidth, newHeight), cv.INTER_AREA)
    return newImage


def trans_k_to_rad(k):
    """
    将斜率转换为弧度
    :param k: 斜率k值
    :return:
    """
    return np.arctan(k)

if __name__ == '__main__':
    filePath = r"D:\2024"
    keepDir = filePath
    if not os.path.isdir(keepDir):
        keepDir, _ = os.path.split(filePath)

    keepOkPath = os.path.join(keepDir, "ok")
    keepOkResPath = os.path.join(keepDir, "res")
    keepFailPath = os.path.join(keepDir, "fail")
    keepFailResPath = os.path.join(keepDir, "fail_res")
    if not os.path.exists(keepOkPath):
        os.makedirs(keepOkPath)
    if not os.path.exists(keepOkResPath):
        os.makedirs(keepOkResPath)
    if not os.path.exists(keepFailPath):
        os.makedirs(keepFailPath)
    if not os.path.exists(keepFailResPath):
        os.makedirs(keepFailResPath)

    width = 1174
    height = 1550
    jointNumber = "6"

    fileNames = []  # 保存要去识别的所有的图片
    if os.path.isfile(filePath):
        filePath, fileName = os.path.split(filePath)
        fileNames.append(fileName)
    else:
        for root, dirs, files in os.walk(filePath):
            if root != filePath:
                continue
            for file in files:
                if file[-4:] != ".bmp":
                    continue
                fileNames.append(file)
    for index, fileName in enumerate(fileNames):
        print("图像名称:{} ".format(fileName))
        filePathReal = os.path.join(filePath, fileName)
        imageTest = cv.imread(filePathReal, cv.IMREAD_GRAYSCALE)
        # print("ImageTest.shape = {},TemplateImage.shape = {}".format(imageTest.shape,TemplateImage.shape))
        # imageSub = cv2.subtract(imageTest,TemplateImage)
        # image_show("ImageSub",imageSub)
        # imageTest = get_size_scale_image(imageTest, 1 / 3)
        imageUsed = get_image_used(imageTest)
        # imageUsed = imageTest.copy()
        calcStart = time.time()
        if jointNumber == "4":
            res, info, center, angle, imageRes,calcSize = find_total_board_info_4pin(imageUsed)
        else:
            res, info, center, angle, imageRes,calcSize = find_total_board_info_6pin(imageUsed)
        if res == "ok":
            print("算法耗时: {}".format(time.time() - calcStart))
            os.remove(filePathReal)
            imageRes = get_size_scale_image(imageRes, sizeScale=4)
            cv.imwrite(os.path.join(keepOkResPath, fileName), imageRes)
            cv.imwrite(os.path.join(keepOkPath, fileName), imageTest)
            print("识别成功")
            continue
        else:
            os.remove(filePathReal)
            cv.imwrite(os.path.join(keepFailPath, fileName), imageTest)
            cv.imwrite(os.path.join(keepFailResPath, fileName), imageRes)
            print("算法耗时: {}".format(time.time() - calcStart))
            print("识别失败:{}".format(info))
